package br.ufrn.dca.observador;

import org.gnu.glpk.GLPK;
import org.gnu.glpk.GLPKConstants;
import org.gnu.glpk.SWIGTYPE_p_double;
import org.gnu.glpk.SWIGTYPE_p_int;
import org.gnu.glpk.glp_prob;
import org.gnu.glpk.glp_smcp;

/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
/**
 *
 * @author allan
 */
//Observador com restrição de erro
public class ObservadorCR extends Observador {

    double eta = 0.1;
    SWIGTYPE_p_double val_p1;
    SWIGTYPE_p_double val_p2;
    SWIGTYPE_p_int ia_p1;
    SWIGTYPE_p_int ia_p2;
    SWIGTYPE_p_int ja_p1;
    SWIGTYPE_p_int ja_p2;
    //Matriz G = [Gf; C; -C]
    double[][] G = {
        {0.281895305322826, -0.959445171355274},
        {0.270230805562446, -0.962795571097557},
        {0.258453970501202, -0.966023573797329},
        {0.246568214455037, -0.969125438537476},
        {0.000000000000001, -1.000000000000000},
        {-0.192433518905760, -0.981310012586006},
        {-0.258453970501270, 0.966023573797311},
        {-0.270230805562418, 0.962795571097565},
        {-0.287684427379640, 0.957725258225577},
        {-0.281895305322733, 0.959445171355301},
        {-0.276077301493646, 0.961135434577243},
        {-0.264356222094113, 0.964425107429358},
        {-0.252524484984084, 0.967590504543902},
        {-0.246568214455039, 0.969125438537475},
        {0.000000000000000, 1.000000000000000},
        {0.252524484984106, -0.967590504543896},
        {0.264356222094059, -0.964425107429373},
        {0.276077301493671, -0.961135434577236},
        {0.192433518905762, 0.981310012586005},
        {0.287684427379629, -0.957725258225581},
        {0, 1}, {0, -1}
    };
    //Matriz rho_eta = [rhof; eta+y; eta-y]
    double[][] rho_eta = {
        {1.390050254993203},
        {1.329493054346401},
        {1.271406369004928},
        {1.215693652992513},
        {1.000000000000001},
        {1.173743531491768},
        {1.271406369005240},
        {1.329493054346245},
        {1.421286573102978},
        {1.390050254992704},
        {1.359456649859709},
        {1.300147024305089},
        {1.243259145479003},
        {1.215693652992515},
        {1.000000000000000},
        {1.243259145479114},
        {1.300147024304839},
        {1.359456649859860},
        {1.173743531491768},
        {1.421286573102924},
        {eta}, {eta}
    };
    //Matriz G_rho = [Gf -rhof]
    double[][] G_rho = new double[G.length - 2][3];
    double[][] Psi = new double[G.length - 2][1];

    public ObservadorCR() {

        val_p1 = GLPK.new_doubleArray(2 * G.length + 1);
        val_p2 = GLPK.new_doubleArray(3 * G_rho.length + 1);

        ia_p1 = GLPK.new_intArray(2 * G.length + 1);
        ia_p2 = GLPK.new_intArray(3 * G_rho.length + 1);

        ja_p1 = GLPK.new_intArray(2 * G.length + 1);
        ja_p2 = GLPK.new_intArray(3 * G_rho.length + 1);

        construirMatriz_G_Rho();
        construirTabelaDeRestricao_P1();
        construirTabelaDeRestricao_P2();
    }

    @Override
    public void calcularObservador(double nivelTanque1, double nivelTanque2) {
        rho_eta[rho_eta.length - 2][0] = eta + (nivelTanque2 - getYeq() - getDeltaX2());
        rho_eta[rho_eta.length - 1][0] = eta - (nivelTanque2 - getYeq() - getDeltaX2());
        LP1();
        LP2();

        //DeltaXc(k+1) = Ad*DeltaXc(k) + Bd*DeltaU(k) - V(z(k))

        setDeltaX((getAd()[0][0] * getDeltaX1() + getBd()[0][0] * getDeltaU() - getV1()),
                (getAd()[1][0] * getDeltaX1() + getAd()[1][1] * getDeltaX2() + getBd()[1][0] * getDeltaU() - getV2()));

        //Xc(k) = Xeq + DeltaXc(k)
        setXck(getXeq1() + getDeltaX1(), getXeq2() + getDeltaX2());

        // Erro = NivelTanqueReal - NivelEstimado
        setErro(nivelTanque1 - getXck1(), nivelTanque2 - getXck2());
    }

    @Override
    public void run(double nivelTanque1, double nivelTanque2) {
        //if (nivelTanque2 >= 14.8) {
            calcularObservador(nivelTanque1, nivelTanque2);
        //        }
    }

    private void LP1() {
        for (int i = 0; i <= G.length - 3; i++) {
            resolve_P1(i);
        }
    }

    private void resolve_P1(int i) {
        glp_prob lp = GLPK.glp_create_prob();

        //definindo restrição
        //rho_eta = [rho;eta+y;eta-y]
        GLPK.glp_add_rows(lp, rho_eta.length);
        for (int j = 0; j <= rho_eta.length - 1; j++) {
            GLPK.glp_set_row_bnds(lp, j + 1, GLPKConstants.GLP_UP, 0, rho_eta[j][0]);
        }
        //definindo objetivo
        //objetivo -> max(Gi*Ad)
        GLPK.glp_set_obj_dir(lp, GLPKConstants.GLP_MAX);
        GLPK.glp_add_cols(lp, 2);
        GLPK.glp_set_col_name(lp, 1, "e1");
        GLPK.glp_set_col_kind(lp, 1, GLPKConstants.GLP_CV);// seta a coluna 1 como uma variavel continua 
        GLPK.glp_set_col_bnds(lp, 1, GLPKConstants.GLP_FR, 0, 0);
        GLPK.glp_set_obj_coef(lp, 1, (G[i][0] * getAd()[0][0] + G[i][1] * getAd()[1][0]));
        GLPK.glp_set_col_name(lp, 1, "e2");
        GLPK.glp_set_col_kind(lp, 2, GLPKConstants.GLP_CV);
        GLPK.glp_set_col_bnds(lp, 2, GLPKConstants.GLP_FR, 0, 0);
        GLPK.glp_set_obj_coef(lp, 2, (G[i][0] * getAd()[0][1] + G[i][1] * getAd()[1][1]));

        GLPK.glp_load_matrix(lp, 2 * G.length, ia_p1, ja_p1, val_p1);

        //resolvendo modelo
        glp_smcp parm = new glp_smcp();
        GLPK.glp_init_smcp(parm);
        int resp = GLPK.glp_simplex(lp, parm);

        if (resp == 0) {
            Psi[i][0] = GLPK.glp_get_obj_val(lp);
//        System.out.println("Psi: " + Psi[p][0]);
        } else {
            System.out.println("---------------------The problem could not be solved");
        }


        //   deletando problema
        GLPK.glp_delete_prob(lp);
    }

    private void LP2() {
        glp_prob lp = GLPK.glp_create_prob();
        //definindo restrição
        //G_rho = [G -rho]
        GLPK.glp_add_rows(lp, G_rho.length);
//        System.out.println(" ------- Matriz Psi ------");
        for (int i = 0; i <= G_rho.length - 1; i++) {
            GLPK.glp_set_row_bnds(lp, i + 1, GLPKConstants.GLP_UP, 0, -1 * Psi[i][0]);
//            System.out.println(" ------------------------- " + Psi[i][0]);
        }

        //definindo objetivo
        // min -> epsi

        GLPK.glp_set_obj_dir(lp, GLPK.GLP_MIN);

        GLPK.glp_add_cols(lp, 3);

        GLPK.glp_set_col_name(lp, 1, "v1");
        GLPK.glp_set_col_kind(lp, 1, GLPKConstants.GLP_CV);
        GLPK.glp_set_col_bnds(lp, 1, GLPKConstants.GLP_FR, 0, 0);
        GLPK.glp_set_obj_coef(lp, 1, 0);
        GLPK.glp_set_col_name(lp, 2, "v2");
        GLPK.glp_set_col_kind(lp, 2, GLPKConstants.GLP_CV);
        GLPK.glp_set_col_bnds(lp, 2, GLPKConstants.GLP_FR, 0, 0);
        GLPK.glp_set_obj_coef(lp, 2, 0);
        GLPK.glp_set_col_name(lp, 3, "epsi");
        GLPK.glp_set_col_kind(lp, 3, GLPKConstants.GLP_CV);
        GLPK.glp_set_col_bnds(lp, 3, GLPKConstants.GLP_FR, 0, 0);
        GLPK.glp_set_obj_coef(lp, 3, 1);

        GLPK.glp_load_matrix(lp, 3 * G_rho.length, ia_p2, ja_p2, val_p2);
        //relovendo lp
        glp_smcp parm = new glp_smcp();
        GLPK.glp_init_smcp(parm);

        int resp = GLPK.glp_simplex(lp, parm);

        if (resp == 0) {
            setV1(GLPK.glp_get_col_prim(lp, 1));
            setV2(GLPK.glp_get_col_prim(lp, 2));
            System.out.println(GLPK.glp_get_col_name(lp, 1) + ": " + GLPK.glp_get_col_prim(lp, 1));
            System.out.println(GLPK.glp_get_col_name(lp, 2) + ": " + GLPK.glp_get_col_prim(lp, 2));
            System.out.println(GLPK.glp_get_col_name(lp, 3) + ": " + GLPK.glp_get_col_prim(lp, 3));
        } else {
            System.out.println("-----------------------------------------The problem could not be solved");
        }
    }

    private void construirMatriz_G_Rho() {
        for (int i = 0; i <= G_rho.length - 1; i++) {
            G_rho[i][0] = G[i][0];
            G_rho[i][1] = G[i][1];
        }
        for (int i = 0; i <= G_rho.length - 1; i++) {
            G_rho[i][2] = -1 * rho_eta[i][0];
        }

//        for (int i = 0; i <= G_rho.length - 1; i++) {
//            for (int j = 0; j <= 2; j++) {
//                System.out.print(G_rho[i][j]);
//                System.out.print("   ");
//            }
//            System.out.println("");
//        }
    }

    private void construirTabelaDeRestricao_P1() {
        int n_linhas = G.length;
        int n_colunas = 2;
        //constroi vetor que representa os indices das linhas da matriz de restrições
        for (int i = 1; i <= n_linhas; i++) {
            for (int j = 1; j <= n_colunas; j++) {
                GLPK.intArray_setitem(ia_p1, 2 * (i - 1) + j, i);
//                System.out.println("Posição: " + (2 * (i - 1) + j) + " valor: " + GLPK.intArray_getitem(ia_p1, 2 * (i - 1) + j));
            }
        }
        //constroi vetor que reprsenta os indices das colunas da matriz de restiçoes
        for (int i = 1; i <= n_linhas; i++) {
            for (int j = 1; j <= n_colunas; j++) {
                GLPK.intArray_setitem(ja_p1, 2 * (i - 1) + j, j);
//                System.out.println("Posição: " + (2 * (i - 1) + j) + " valor: " + GLPK.intArray_getitem(ja_p1, 2*(i-1)+j));
            }
        }
        //constroi vetor com valores correspondente a cada indice da matriz de restriçoes
        for (int i = 1; i <= n_linhas; i++) {
            for (int j = 1; j <= n_colunas; j++) {
                GLPK.doubleArray_setitem(val_p1, 2 * (i - 1) + j, G[i - 1][j - 1]);
//                System.out.println("Posição: " + (2 * (i - 1) + j) + " valor: " + GLPK.doubleArray_getitem(val_p1, 2 * (i - 1) + j));
            }
        }
    }

    private void construirTabelaDeRestricao_P2() {
        int n_linhas = G_rho.length;
        int n_colunas = 3;

        for (int i = 1; i <= n_linhas; i++) {
            for (int j = 1; j <= n_colunas; j++) {
                GLPK.intArray_setitem(ia_p2, 3 * (i - 1) + j, i);
//                System.out.println((3 * (i - 1) + j) + "----" + GLPK.intArray_getitem(ia_p2, 3 * (i - 1) + j));
            }
        }

        for (int i = 1; i <= n_linhas; i++) {
            for (int j = 1; j <= n_colunas; j++) {
                GLPK.intArray_setitem(ja_p2, 3 * (i - 1) + j, j);
//                System.out.println((3 * (i - 1) + j) + "----" + GLPK.intArray_getitem(ja_p2, 3 * (i - 1) + j));
            }
        }

        for (int i = 1; i <= n_linhas; i++) {
            for (int j = 1; j <= n_colunas; j++) {
                GLPK.doubleArray_setitem(val_p2, 3 * (i - 1) + j, G_rho[i - 1][j - 1]);
//              System.out.println("Posição: " + (n_colunas*(i-1) + j) + " valor: " + GLPK.doubleArray_getitem(val_p2, (n_colunas*(i-1) + j)));
            }
        }
    }
}
